/*-----------------------------------------------------------------------
|                            FILE DESCRIPTION                           |
-----------------------------------------------------------------------*/
/*----------------------------------------------------------------------
  - File name     : iMath.h
  - Author        : liuzhihua
  - Update date   : 2022.4.7
  -	File Function : Mathematical tool library
-----------------------------------------------------------------------*/
/*-----------------------------------------------------------------------
|                               UPDATE NOTE                             |
-----------------------------------------------------------------------*/
/**
 * Update note:
 * ------------   ---------------   ----------------------------------
 *     Date            Author                       Note
 * ------------   ---------------   ----------------------------------
 *   2022.4.7        liuzhihua                   Create file
 ***/

#ifndef __IMATH_H_
#define __IMATH_H_

#ifdef __cplusplus
extern "C"
{
#endif
  /*-----------------------------------------------------------------------
  |                               INCLUDES                                |
  -----------------------------------------------------------------------*/

#include "stdarg.h"
#include "string.h"
#include "stdio.h"
#include "math.h"
#include "stdlib.h"
#include "stdint.h"
#include "polyfit.h"
/*-----------------------------------------------------------------------
|                                DEFINES                                |
-----------------------------------------------------------------------*/
#define iSWAP(temp, a, b) \
{                     \
	temp = (a);       \
	(a) = (b);        \
	(b) = temp;       \
}

typedef struct kalman1_t
{
	float x;//最优估计
	float p;//估计值的协方差
	float K;//卡尔曼增益
	float q;//过程噪声协方差
	float r;//测量噪声协方差(传感器测量误差)
	float A;//状态转移矩阵
	float H;//观测模型(将观测值转换为估计值，单位换算)

} kalman1_t;
#define KALMAN1_INIT(init_x, init_p, Q, R) \
	{                                      \
		init_x,                       \
		init_p,                       \
        0,                            \
		Q,/*过程噪声*/                 \
		R,/*测量噪声*/                 \
		1,                            \
		1,                            \
	}


#define ARR_MOVE_LEFT(arr, size) \
    do                           \
    {                            \
        int i = 0;               \
        while (i < size - 1)     \
        {                        \
            arr[i] = arr[i + 1]; \
            i++;                 \
        }                        \
    } while (0)

#define ARR_MOVE_RIGHT(arr, size) \
    do                            \
    {                             \
        int i = size - 1;         \
        while (i)                 \
        {                         \
            arr[i] = arr[i - 1];  \
            i--;                  \
        }                         \
    } while (0)

#define SING_BIT(x) (1 << (sizeof(x) * 8 - 1))
#define SING_GET(x) (int)((x & SING_BIT(x)) != 0 ? -1 : 1)
/*-----------------------------------------------------------------------
|                             API FUNCTION                              |
-----------------------------------------------------------------------*/
void bubble_sort(int *array, size_t num);
void xybubble_sort(int *x, int *y, size_t num);
void xybubble_sort_u16(uint16_t *x, uint16_t *y, size_t num);
void insertion_sort(int *array, int num);
void shell_sort(int *array, int num);
void qsort_int(void *base, size_t Long);
void qsort_uint8(void *base, size_t Long);
void qsort_uint16(void *base, size_t Long);
void qsort_uint32(void *base, size_t Long);
size_t FindMaxIndex(int *data, size_t length);
size_t FindMinIndex(int *data, size_t length);
int FindMedian(int arr[], int n);
double line_xyxy_GetX(double x0, double y0, double x1, double y1, double y);
double line_xyxy_GetY(double x0, double y0, double x1, double y1, double x);
long sum_int32(int *arr, size_t size);
long sum_int16(int16_t *arr, size_t size);
uint64_t sum_u16(uint16_t *arr, size_t size);
double filter_average_int32(int *arr, size_t size);
double filter_average_int16(int16_t *arr, size_t size);
double filter_average_u16(uint16_t *arr, size_t size);
void kalman1_init(kalman1_t* pkm1, float X, float P, float Q, float R);
float kalman1_filter(kalman1_t *state, float z_measure);
#ifdef __cplusplus
}
#endif

#endif
